
#include "btBulletCollisionCommon.h"
#include "btBulletDynamicsCommon.h"
#include "Interop/Interop.h"
#include "Interop/Log.h"
#include "BulletFacade.h"

btRigidBody* LocalCreateRigidBody(btCollisionShape* shape,
								const btScalar *glOrientation,
								const btScalar& mass)
{
	// rigid body is dynamic if and only if mass is non zero, otherwise static
	btVector3 localInertia(0, 0, 0);
	bool isDynamic = (mass != 0.f);
	if (isDynamic)
	{
		shape->calculateLocalInertia(mass, localInertia);
	}

	btTransform transform;
	transform.setIdentity();
	transform.setFromOpenGLMatrix(glOrientation);

	btDefaultMotionState* motionState = new btDefaultMotionState(transform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, motionState, shape, localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);
	LOGE("Created rigid body with address: %d", body);
	return body;
}

//////////////////////////////////////////////////////////////////////////

JNIEXPORT
jint
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_createDiscreteDynamicsWorldNative(
		JNIEnv* env,
		jobject owner,
		jobject gravityVector)
{
	// default collision configuration
	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();

	// default collision dispatcher
	btCollisionDispatcher* dispatcher = new	btCollisionDispatcher(collisionConfiguration);

	btDbvtBroadphase* overlappingPairCache = new btDbvtBroadphase();

	// the default constraint solver
	btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;

	btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration);
	btVector3 gravity = InteropGetVector3(env, gravityVector);
	dynamicsWorld->setGravity(gravity);
	return (int)dynamicsWorld;
}

JNIEXPORT
jboolean
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_setRigidBodyOrientationNative(
		JNIEnv* env,
		jobject owner,
		jint bodyAddress,
		jfloatArray glOrientMatrix)
{
	if (bodyAddress == 0) {
		return JNI_FALSE;
	}

	btRigidBody* body = (btRigidBody*) bodyAddress;
	btTransform trans;
	jfloat *glArray = env->GetFloatArrayElements(glOrientMatrix, 0);

	// glArray must have 16 elements
	trans.setFromOpenGLMatrix((btScalar*) glArray);
	env->ReleaseFloatArrayElements(glOrientMatrix, glArray, 0);
	body->getMotionState()->setWorldTransform(trans);
	return JNI_TRUE;
}

JNIEXPORT
jint
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_createRigidBodyNative(
		JNIEnv* env,
		jobject owner,
		jint worldAddress,
		jint shapeAddress,
		jfloatArray glOrientMatrix,
		jfloat mass,
		jint collisionGroup,
		jint collisionMask,
		jboolean useDefaultMasks)
{
	if (worldAddress == 0 || shapeAddress == 0) {
		return 0;
	}

	btCollisionShape* shape = (btCollisionShape*) shapeAddress;

	// glArray must have 16 elements
	jfloat *glArray = env->GetFloatArrayElements(glOrientMatrix, 0);
	btRigidBody* body = LocalCreateRigidBody(shape, (btScalar*)glArray, btScalar(mass));
	env->ReleaseFloatArrayElements(glOrientMatrix, glArray, 0);
	btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) worldAddress;

	if (useDefaultMasks == JNI_TRUE) {
//		LOGD("using default masks");
		world->addRigidBody(body);
	}
	else {
//		LOGD("using user-defined masks: %d, %d", collisionGroup, collisionMask);
		world->addRigidBody(body, short(collisionGroup), short(collisionMask));
	}
	return (int) body;
}

JNIEXPORT
jint
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_createPoint2PointConstraintNative(
		JNIEnv* env,
		jobject owner,
		jint worldAddress,
		jint bodyAddress,
		jobject pivotVector)
{
	if (worldAddress == 0 || bodyAddress == 0) {
		return 0;
	}

	btRigidBody* body = (btRigidBody*) bodyAddress;
	btVector3 pivot = InteropGetVector3(env, pivotVector);
	btTypedConstraint* p2p = new btPoint2PointConstraint(*body, pivot);

	btDynamicsWorld* world = (btDynamicsWorld*) worldAddress;
	world->addConstraint(p2p);
	return (int) p2p;
}

JNIEXPORT
jint
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_createPoint2PointConstraint2BodiesNative(
		JNIEnv* env,
		jobject owner,
		jint worldAddress,
		jint body1Address,
		jint body2Address,
		jobject pivot1Vector,
		jobject pivot2Vector)
{
	if (worldAddress == 0 || body1Address == 0 || body2Address == 0) {
		return 0;
	}

	btRigidBody* body1 = (btRigidBody*) body1Address;
	btRigidBody* body2 = (btRigidBody*) body2Address;
	btVector3 pivot1 = InteropGetVector3(env, pivot1Vector);
	btVector3 pivot2 = InteropGetVector3(env, pivot2Vector);
	btTypedConstraint* constraint = new btPoint2PointConstraint(*body1, *body2, pivot1, pivot2);

	btDynamicsWorld* world = (btDynamicsWorld*) worldAddress;
	world->addConstraint(constraint);
	return (int) constraint;
}

JNIEXPORT
jint
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_createHingeConstraintNative(
		JNIEnv* env,
		jobject owner,
		jint worldAddress,
		jint bodyAddress,
		jobject pivotVector,
		jobject axisVector)
{
	if (worldAddress == 0 || bodyAddress == 0) {
		return 0;
	}

	btRigidBody* body = (btRigidBody*) bodyAddress;
	btVector3 pivot = InteropGetVector3(env, pivotVector);
	btVector3 axis = InteropGetVector3(env, axisVector);
	btTypedConstraint* constraint = new btHingeConstraint(*body, pivot, axis);

	btDynamicsWorld* world = (btDynamicsWorld*) worldAddress;
	world->addConstraint(constraint);
	return (int) constraint;
}

JNIEXPORT
jint
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_createSliderConstraint2BodiesNative(
		JNIEnv* env,
		jobject owner,
		jint worldAddress,
		jint body1Address,
		jint body2Address,
		jfloatArray transformsMatrix)
{
	if (worldAddress == 0 || body1Address == 0 || body2Address == 0) {
		return 0;
	}

	btTransform transform1 = btTransform::getIdentity();
	jfloat *transformMatrixArray = env->GetFloatArrayElements(transformsMatrix, 0);
	transform1.setFromOpenGLMatrix(transformMatrixArray);

	btTransform transform2 = btTransform::getIdentity();
	transform2.setFromOpenGLMatrix(transformMatrixArray + 16);
	env->ReleaseFloatArrayElements(transformsMatrix, transformMatrixArray, 0);

	btRigidBody* body1 = (btRigidBody*) body1Address;
	btRigidBody* body2 = (btRigidBody*) body2Address;
	btTypedConstraint* constraint = new btSliderConstraint(*body1, *body2, transform1, transform2, true);

	btDynamicsWorld* world = (btDynamicsWorld*) worldAddress;
	world->addConstraint(constraint);
	return (int) constraint;
}

JNIEXPORT
jint
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_createSliderConstraintNative(
		JNIEnv* env,
		jobject owner,
		jint worldAddress,
		jint body1Address,
		jfloatArray transformMatrix1,
		jfloat lowerLinearLimit,
		jfloat upperLinearLimit)
{
	if (worldAddress == 0 || body1Address == 0) {
		return 0;
	}

	btTransform transform1 = btTransform::getIdentity();
	jfloat *transformMatrixArray = env->GetFloatArrayElements(transformMatrix1, 0);
	transform1.setFromOpenGLMatrix(transformMatrixArray);
	env->ReleaseFloatArrayElements(transformMatrix1, transformMatrixArray, 0);

	btRigidBody* body1 = (btRigidBody*) body1Address;
	btSliderConstraint* constraint = new btSliderConstraint(*body1, transform1, true);
	LOGD("Lower: %f", lowerLinearLimit);
	LOGD("Upper: %f", upperLinearLimit);
	constraint->setLowerLinLimit(btScalar(lowerLinearLimit));
	constraint->setUpperLinLimit(btScalar(upperLinearLimit));
	btDynamicsWorld* world = (btDynamicsWorld*) worldAddress;
	world->addConstraint(constraint);
	return (int) constraint;
}

JNIEXPORT
jint
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_createBoxCollisionShapeNative(
		JNIEnv* env,
		jobject owner,
		jfloat xSize, jfloat ySize, jfloat zSize)
{
	btCollisionShape* boxShape = new btBoxShape(btVector3(xSize, ySize, zSize));
	LOGD("BOX_SHAPE_PROXYTYPE: %d", BOX_SHAPE_PROXYTYPE);
	LOGD("shapeType: %d", boxShape->getShapeType());
	return (int) boxShape;
}

JNIEXPORT
jint
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_createSphereCollisionShapeNative(
		JNIEnv* env,
		jobject owner,
		jfloat radius)
{
	btCollisionShape* sphereShape = new btSphereShape(btScalar(radius));
	return (int) sphereShape;
}

JNIEXPORT
jboolean
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_destroyDiscreteDynamicsWorldNative(JNIEnv* env,
																				 	 jobject owner,
																				 	 jint worldDataAddress)
{
	if (worldDataAddress == 0) {
		return JNI_FALSE;
	}

	btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) worldDataAddress;
	delete world;
	return JNI_TRUE;
}

JNIEXPORT
jboolean
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_destroyRigidBodyNative(JNIEnv* env,
																		 jobject owner,
																		 jint worldAddress,
																		 jint rigidBodyAddress)
{
	if (worldAddress == 0 || rigidBodyAddress == 0) {
		return JNI_FALSE;
	}

	btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) worldAddress;
	btRigidBody* body = (btRigidBody*) rigidBodyAddress;
	world->removeRigidBody(body);
	delete body;
	return JNI_TRUE;
}

JNIEXPORT
jboolean
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_destroyCollisionShapeNative(JNIEnv* env,
																			  jobject owner,
																			  jint collisionShapeAddress)
{
	if (collisionShapeAddress == 0) {
		return JNI_FALSE;
	}

	btCollisionShape* collisionShape = (btCollisionShape*) collisionShapeAddress;
	delete collisionShape;
	return JNI_TRUE;
}

JNIEXPORT
jboolean
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_destroyConstraintNative(JNIEnv* env,
																   jobject owner,
																   jint worldAddress,
																   jint constraintAddress)
{
	if (worldAddress == 0 || constraintAddress == 0) {
		return JNI_FALSE;
	}

	btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) worldAddress;
	btTypedConstraint* constraint = (btTypedConstraint*) constraintAddress;
	world->removeConstraint(constraint);
	delete constraint;
	return JNI_TRUE;
}

JNIEXPORT
jboolean
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_doSimulationNative(JNIEnv* env,
																	 jobject owner,
																	 jint worldAddress,
																	 jfloat timeStep,
																	 jint maxSubSteps)
{
	if (worldAddress == 0) {
		return JNI_FALSE;
	}

//	LOGV("doSimulationNative");
	btDynamicsWorld* world = (btDynamicsWorld*) worldAddress;
	world->stepSimulation(timeStep, maxSubSteps);

	//print positions of all objects
	int objectsCount = world->getNumCollisionObjects();

	btCollisionObjectArray& objects = world->getCollisionObjectArray();

	jfloat glMatrix[16];
	jfloatArray javaGLMatrix = env->NewFloatArray(sizeof(glMatrix));
	jclass c = env->GetObjectClass(owner);
	jmethodID callbackMethodID = env->GetMethodID(c, "onRigidBodyUpdate", "(I[F)V");

	for (int i = objectsCount - 1; i >= 0 ; --i)
	{
		btCollisionObject* obj = objects[i];
//		if (!(obj->isActive())) {
//			continue;
//		}

		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			btTransform trans;
			body->getMotionState()->getWorldTransform(trans);

			trans.getOpenGLMatrix(glMatrix);

			env->SetFloatArrayRegion(javaGLMatrix, 0, sizeof(glMatrix) - 1, glMatrix);
			env->CallVoidMethod(owner, callbackMethodID, (int)body, javaGLMatrix);
		}
	}

	return JNI_TRUE;
}

JNIEXPORT
jint
JNICALL
Java_se_lnu_axez_physics_bullet_BulletFacade_pickBodyNative(JNIEnv* env,
																	 jobject owner,
																	 jint worldAddress,
																	 jfloatArray rayCoord,
																	 jint nearOffset,
																	 jint farOffset)
{
	if (worldAddress == 0) {
		return 0;
	}

	jfloat *coordArray = env->GetFloatArrayElements(rayCoord, 0);
	btVector3 rayFrom = btVector3(coordArray[nearOffset],
								  coordArray[nearOffset + 1],
								  coordArray[nearOffset + 2]);
	btVector3 rayTo = btVector3(coordArray[farOffset],
								coordArray[farOffset + 1],
								coordArray[farOffset + 2]);
	env->ReleaseFloatArrayElements(rayCoord, coordArray, 0);

//	LOGD("pick:");
//	LOGD("from: %f, %f, %f", rayFrom.getX(), rayFrom.getY(), rayFrom.getZ());
//	LOGD("to: %f, %f, %f", rayTo.getX(), rayTo.getY(), rayTo.getZ());

	btDynamicsWorld* world = (btDynamicsWorld*) worldAddress;
	btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom, rayTo);
	world->rayTest(rayFrom, rayTo, rayCallback);
	if (!rayCallback.hasHit()) {
//		LOGD("nothing picked");
		return 0;
	}

//	LOGD("body picked: %d", rayCallback.m_collisionObject);
	return (int) rayCallback.m_collisionObject;
}
